#include "G2oTypes.h"
#include "ImuTypes.h"
#include "Converter.h"

namespace msf
{

/** 
 * @brief 储存关键帧位姿相关的信息，用于优化
 * @param pKPF 关键帧
 */  
ImuOdomPose::ImuOdomPose(KeyPoseFrame *pKPF):its(0)
{
    // Load IMU pose
    twb = pKPF->GetImuPosition().cast<double>();
    Rwb = pKPF->GetImuRotation().cast<double>();

    // Load odom pose
    int num_odoms=1;

    tow.resize(num_odoms);
    Row.resize(num_odoms);
    tob.resize(num_odoms);
    Rob.resize(num_odoms);
    Rbo.resize(num_odoms);
    tbo.resize(num_odoms);

    // odom
    tow[0] = pKPF->GetTranslation().cast<double>();
    Row[0] = pKPF->GetRotation().cast<double>();
    tob[0] = pKPF->mImuCalib.mTob.translation().cast<double>();
    Rob[0] = pKPF->mImuCalib.mTob.rotationMatrix().cast<double>();
    Rbo[0] = Rob[0].transpose();
    tbo[0] = pKPF->mImuCalib.mTbo.translation().cast<double>();

    // For posegraph 4DoF
    Rwb0 = Rwb;
    DR.setIdentity();
}

/** 
 * @brief 储存普通帧位姿相关的信息，用于优化
 * @param pPF 普通帧
 */
ImuOdomPose::ImuOdomPose(PoseFrame *pPF):its(0)
{
    // Load IMU pose
    twb = pPF->GetImuPosition().cast<double>();
    Rwb = pPF->GetImuRotation().cast<double>();

    // Load camera poses
    int num_odoms=1;

    tow.resize(num_odoms);
    Row.resize(num_odoms);
    tob.resize(num_odoms);
    Rob.resize(num_odoms);
    Rbo.resize(num_odoms);
    tbo.resize(num_odoms);

    // odom
    tow[0] = pPF->GetPose().translation().cast<double>();
    Row[0] = pPF->GetPose().rotationMatrix().cast<double>();
    tob[0] = pPF->mImuCalib.mTob.translation().cast<double>();
    Rob[0] = pPF->mImuCalib.mTob.rotationMatrix().cast<double>();
    Rbo[0] = Rob[0].transpose();
    tbo[0] = pPF->mImuCalib.mTbo.translation().cast<double>();

    // For posegraph 4DoF
    Rwb0 = Rwb;
    DR.setIdentity();
}

/** 
 * @brief 储存位姿相关的信息，用于优化
 */
ImuOdomPose::ImuOdomPose(Eigen::Matrix3d &_Rwo, Eigen::Vector3d &_two, KeyPoseFrame* pKPF): its(0)
{
    // This is only for posegrpah, we do not care about multicamera
    tow.resize(1);
    Row.resize(1);
    tob.resize(1);
    Rob.resize(1);
    Rbo.resize(1);
    tbo.resize(1);

    tob[0] = pKPF->mImuCalib.mTob.translation().cast<double>();
    Rob[0] = pKPF->mImuCalib.mTob.rotationMatrix().cast<double>();
    Rbo[0] = Rob[0].transpose();
    tbo[0] = pKPF->mImuCalib.mTbo.translation().cast<double>();
    // 计算IMU在世界坐标系下的旋转矩阵Rwb和平移向量twb
    twb = _Rwo * tob[0] + _two;
    Rwb = _Rwo * Rob[0];
    // odom在世界坐标系下的旋转矩阵Row[0]和平移向量tow[0]
    Row[0] = _Rwo.transpose();
    tow[0] = -Row[0] * _two;

    // For posegraph 4DoF
    Rwb0 = Rwb;
    DR.setIdentity();
}

/** 
 * @brief 设置相关数据
 */
void ImuOdomPose::SetParam(
    const std::vector<Eigen::Matrix3d> &_Row, const std::vector<Eigen::Vector3d> &_tow,
    const std::vector<Eigen::Matrix3d> &_Rbo, const std::vector<Eigen::Vector3d> &_tbo)
{
    Rbo = _Rbo;
    tbo = _tbo;
    Row = _Row;
    tow = _tow;
    const int num_odoms = Rbo.size();
    Rob.resize(num_odoms);
    tob.resize(num_odoms);

    for(int i=0; i<tob.size(); i++)
    {
        Rob[i] = Rbo[i].transpose();
        tob[i] = -Rob[i]*tbo[i];
    }
    Rwb = Row[0].transpose()*Rob[0];
    twb = Row[0].transpose()*(tob[0]-tow[0]);
}

/** 
 * @brief 优化算出更新值，更新到状态中
 * @param pu 更新值
 */
void ImuOdomPose::Update(const double *pu)
{
    Eigen::Vector3d ur, ut;
    // Problem15.1 直接将imu系的y和z轴pose更新置0
    ur << pu[0], pu[1], pu[2];
    //ut << pu[3], pu[4], pu[5];
    ut << pu[3], 0.0, 0.0;

    // Update body pose
    // 更新的是imu位姿
    twb += Rwb * ut;
    Rwb = Rwb * ExpSO3(ur);

    // Normalize rotation after 5 updates
    its++;
    if(its>=3)
    {
        NormalizeRotation(Rwb);
        its=0;
    }

    // Update odom poses
    const Eigen::Matrix3d Rbw = Rwb.transpose();
    const Eigen::Vector3d tbw = -Rbw * twb;

    Row[0] = Rob[0] * Rbw;
    tow[0] = Rob[0] * tbw + tob[0];
}

// 更新世界坐标系
void ImuOdomPose::UpdateW(const double *pu)
{
    // 旋转和平移的增量
    Eigen::Vector3d ur, ut;
    ur << pu[0], pu[1], pu[2];
    ut << pu[3], pu[4], pu[5];

    // DR: 从初始位姿到当前位姿的旋转变换
    const Eigen::Matrix3d dR = ExpSO3(ur);
    DR = dR * DR;

    // DR乘以初始位姿Rwb0，得到新的Rwb。Rwb是IMU在世界坐标系下的旋转矩阵
    Rwb = DR * Rwb0;
    // ut加到twb上，得到新的twb。twb是IMU在世界坐标系下的平移向量
    twb += ut;

    // Normalize rotation after 5 updates
    // 保证旋转矩阵的正交性
    its++;
    if(its>=5)
    {
        DR(0,2) = 0.0;
        DR(1,2) = 0.0;
        DR(2,0) = 0.0;
        DR(2,1) = 0.0;
        NormalizeRotation(DR);
        its = 0;
    }

    // Update odom pose
    // 首先求出IMU在世界坐标系下的旋转矩阵Rbw和平移向量tbw
    const Eigen::Matrix3d Rbw = Rwb.transpose();
    const Eigen::Vector3d tbw = -Rbw * twb;

    // 对于odom，乘以IMU到odom的旋转矩阵Rob和平移向量tob，得到odom在世界坐标系下的旋转矩阵Row和平移向量tow
    Row[0] = Rob[0] * Rbw;
    tow[0] = Rob[0] * tbw+tob[0];
}

/** 
 * @brief 写入状态量
 */
bool VertexPose::read(std::istream& is)
{
    // odom的世界坐标系到odom坐标系的旋转矩阵Row和平移向量tow
    std::vector<Eigen::Matrix<double,3,3> > Row;
    std::vector<Eigen::Matrix<double,3,1> > tow;
    // odom到IMU的旋转矩阵Rbo和平移向量tbo
    std::vector<Eigen::Matrix<double,3,3> > Rbo;
    std::vector<Eigen::Matrix<double,3,1> > tbo;

    // odom的数量num_odoms，用于循环读取每个odom的状态量
    const int num_odoms = _estimate.Rbo.size();
    for(int idx = 0; idx<num_odoms; idx++)
    {

        for (int i=0; i<3; i++){
            for (int j=0; j<3; j++)
                is >> Row[idx](i,j);
        }

        for (int i=0; i<3; i++){
            is >> tow[idx](i);
        }

        for (int i=0; i<3; i++){
            for (int j=0; j<3; j++)
                is >> Rbo[idx](i,j);
        }

        for (int i=0; i<3; i++){
            is >> tbo[idx](i);
        }
    }

    _estimate.SetParam(Row,tow,Rbo,tbo);
    updateCache();
    
    return true;
}

/** 
 * @brief 读出状态量
 */
bool VertexPose::write(std::ostream& os) const
{
    std::vector<Eigen::Matrix<double,3,3> > Row = _estimate.Row;
    std::vector<Eigen::Matrix<double,3,1> > tow = _estimate.tow;

    std::vector<Eigen::Matrix<double,3,3> > Rbo = _estimate.Rbo;
    std::vector<Eigen::Matrix<double,3,1> > tbo = _estimate.tbo;

    const int num_odoms = tow.size();

    for(int idx = 0; idx<num_odoms; idx++)
    {
        for (int i=0; i<3; i++){
            for (int j=0; j<3; j++)
                os << Row[idx](i,j) << " ";
        }
        for (int i=0; i<3; i++){
            os << tow[idx](i) << " ";
        }

        for (int i=0; i<3; i++){
            for (int j=0; j<3; j++)
                os << Rbo[idx](i,j) << " ";
        }
        for (int i=0; i<3; i++){
            os << tbo[idx](i) << " ";
        }
    }

    return os.good();
}

VertexVelocity::VertexVelocity(KeyPoseFrame* pKPF)
{
    setEstimate(pKPF->GetVelocity().cast<double>());
}

VertexVelocity::VertexVelocity(PoseFrame* pPF)
{
    setEstimate(pPF->GetVelocity().cast<double>());
}

VertexGyroBias::VertexGyroBias(KeyPoseFrame *pKPF)
{
    setEstimate(pKPF->GetGyroBias().cast<double>());
}

VertexGyroBias::VertexGyroBias(PoseFrame *pPF)
{
    Eigen::Vector3d bg;
    bg << pPF->mImuBias.bwx, pPF->mImuBias.bwy,pPF->mImuBias.bwz;
    setEstimate(bg);
}

VertexAccBias::VertexAccBias(KeyPoseFrame *pKPF)
{
    setEstimate(pKPF->GetAccBias().cast<double>());
}

VertexAccBias::VertexAccBias(PoseFrame *pPF)
{
    Eigen::Vector3d ba;
    ba << pPF->mImuBias.bax, pPF->mImuBias.bay,pPF->mImuBias.baz;
    setEstimate(ba);
}

/** 
 * @brief IMU + ODOM惯性测量边（此时已经初始化完毕不需要再优化重力方向与尺度）
 * @param pInt 预积分相关内容
 */
EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast<double>()),
    JVg(pInt->JVg.cast<double>()), JPg(pInt->JPg.cast<double>()), JVa(pInt->JVa.cast<double>()),
    JPa(pInt->JPa.cast<double>()), mpInt(pInt), dt(pInt->dT)
{
    // 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的
    // This edge links 6 vertices
    // 6元边
    resize(6);
    // 1. 定义重力
    g << 0, 0, -IMU::GRAVITY_VALUE;

    // 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵
    Matrix9d Info = pInt->C.block<9,9>(0,0).cast<double>().inverse();

    // 3. 强制让其成为对角矩阵
    Info = (Info+Info.transpose())/2;

    // 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧）
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9,9> > es(Info);
    Eigen::Matrix<double,9,1> eigs = es.eigenvalues();
    for(int i=0;i<9;i++)
        if(eigs[i]<1e-12)
            eigs[i]=0;

    // asDiagonal 生成对角矩阵
    Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
    setInformation(Info);
}

/** 
 * @brief 计算误差
 */
void EdgeInertial::computeError()
{
    // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);           //位姿Ti
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);    //速度vi
    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);    //零偏Bgi
    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);      //零偏Bai
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);           //位姿Tj
    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);   //速度vj
    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();
    const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast<double>();
    const Eigen::Vector3d dP = mpInt->GetDeltaPosition(b1).cast<double>();

    Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
    Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(VV2->estimate() - VV1->estimate() - g*dt) - dV;
    Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb - VV1->estimate()*dt - g*dt*dt/2) - dP;

    // for debug
    dp = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb - VV1->estimate()*dt - g*dt*dt/2);
    dv = VP1->estimate().Rwb.transpose()*(VV2->estimate() - VV1->estimate() - g*dt);

    ba << VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2];
    bg << VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2];

    DP = mpInt->GetDeltaPosition(b1).cast<double>();
    DV = mpInt->GetDeltaVelocity(b1).cast<double>();

    eR = er;
    eV = ev;
    eP = ep;

    _error << er, ev, ep;
}

// 计算雅克比矩阵
void EdgeInertial::linearizeOplus()
{
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2= static_cast<const VertexVelocity*>(_vertices[5]);
    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
    const IMU::Bias db = mpInt->GetDeltaBias(b1);
    Eigen::Vector3d dbg;
    dbg << db.bwx, db.bwy, db.bwz;

    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;  // Ri
    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();     // Ri.t()
    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;  // Rj

    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();
    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;        // r△Rij
    const Eigen::Vector3d er = LogSO3(eR);                      // r△φij
    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);  // Jr^-1(log(△Rij))

    // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度
    // Jacobians wrt Pose 1
    // _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导
    _jacobianOplus[0].setZero();
    // rotation
    // (0,0)起点的3*3块表示旋转残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1; // OK
    // (3,0)起点的3*3块表示速度残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt)); // OK
    // (6,0)起点的3*3块表示位置残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(VP2->estimate().twb - VP1->estimate().twb
                                                   - VV1->estimate()*dt - 0.5*g*dt*dt)); // OK
    // translation
    // (6,3)起点的3*3块表示位置残差对pose1的位置求导
    _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity(); // OK

    // Jacobians wrt Velocity 1
    // _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导
    _jacobianOplus[1].setZero();
    _jacobianOplus[1].block<3,3>(3,0) = -Rbw1; // OK
    _jacobianOplus[1].block<3,3>(6,0) = -Rbw1*dt; // OK

    // Jacobians wrt Gyro 1
    // _jacobianOplus[2] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导
    _jacobianOplus[2].setZero();
    _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg; // OK
    _jacobianOplus[2].block<3,3>(3,0) = -JVg; // OK
    _jacobianOplus[2].block<3,3>(6,0) = -JPg; // OK

    // Jacobians wrt Accelerometer 1
    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导
    _jacobianOplus[3].setZero();
    _jacobianOplus[3].block<3,3>(3,0) = -JVa; // OK
    _jacobianOplus[3].block<3,3>(6,0) = -JPa; // OK

    // Jacobians wrt Pose 2
    // _jacobianOplus[4] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
    _jacobianOplus[4].setZero();
    // rotation
    _jacobianOplus[4].block<3,3>(0,0) = invJr; // OK
    // translation
    _jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2; // OK

    // Jacobians wrt Velocity 2
    // _jacobianOplus[5] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
    _jacobianOplus[5].setZero();
    _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK
}

// localoptimizing中imu初始化所用的边，除了正常的几个优化变量外还优化了 重力方向 与 尺度
EdgeInertialGS::EdgeInertialGS(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast<double>()),
    JVg(pInt->JVg.cast<double>()), JPg(pInt->JPg.cast<double>()), JVa(pInt->JVa.cast<double>()),
    JPa(pInt->JPa.cast<double>()), mpInt(pInt), dt(pInt->dT)
{
    // 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的
    // This edge links 8 vertices
    // 8元边
    resize(8);
    // 1. 定义重力
    gI << 0, 0, -IMU::GRAVITY_VALUE;

    // 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵
    Matrix9d Info = pInt->C.block<9,9>(0,0).cast<double>().inverse();
    // 3. 强制让其成为对角矩阵
    Info = (Info+Info.transpose())/2;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9,9> > es(Info);
    // 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧）
    Eigen::Matrix<double,9,1> eigs = es.eigenvalues();
    for(int i=0;i<9;i++)
        if(eigs[i]<1e-12)
            eigs[i]=0;
    // asDiagonal 生成对角矩阵
    Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
    setInformation(Info);
}

// 计算误差
void EdgeInertialGS::computeError()
{
    // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
    const VertexGDir* VGDir = static_cast<const VertexGDir*>(_vertices[6]);
    const VertexScale* VS = static_cast<const VertexScale*>(_vertices[7]);

    const IMU::Bias b(VA->estimate()[0],VA->estimate()[1],VA->estimate()[2],VG->estimate()[0],VG->estimate()[1],VG->estimate()[2]);
    
    //g = VGDir->estimate().Rwg*gI;
    // Problem 20 estimation of g is not accurate??
    g = gI;
    const double s = VS->estimate();
    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b).cast<double>();
    const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b).cast<double>();
    const Eigen::Vector3d dP = mpInt->GetDeltaPosition(b).cast<double>();

    // 计算残差。广义上讲都是真实值 = 残差 + imu，旋转为imu*残差=真实值
    // dR.transpose() 为imu预积分的值，VP1->estimate().Rwb.transpose() * VP2->estimate().Rwb 为相机的Rwc在乘上相机与imu的标定外参矩阵
    const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
    const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(s*(VV2->estimate() - VV1->estimate()) - g*dt) - dV;
    const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(s*(VP2->estimate().twb - VP1->estimate().twb - VV1->estimate()*dt) - g*dt*dt/2) - dP;

    // for debug eP = dp - dP
    dp = VP1->estimate().Rwb.transpose()*(s*(VP2->estimate().twb - VP1->estimate().twb - VV1->estimate()*dt) - g*dt*dt/2);
    dv = VP1->estimate().Rwb.transpose()*(s*(VV2->estimate() - VV1->estimate()) - g*dt);

    DP = mpInt->GetDeltaPosition(b).cast<double>();
    DV = mpInt->GetDeltaVelocity(b).cast<double>();

    ba << VA->estimate()[0],VA->estimate()[1],VA->estimate()[2];
    bg << VG->estimate()[0],VG->estimate()[1],VG->estimate()[2];

    eR = er;
    eV = ev;
    eP = ep;

    _error << er, ev, ep;
}

// 计算雅克比矩阵
void EdgeInertialGS::linearizeOplus()
{
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
    const VertexGDir* VGDir = static_cast<const VertexGDir*>(_vertices[6]);
    const VertexScale* VS = static_cast<const VertexScale*>(_vertices[7]);
    // 1. 获取偏置变量，因为要对这个东西求导
    const IMU::Bias b(VA->estimate()[0],VA->estimate()[1],VA->estimate()[2],VG->estimate()[0],VG->estimate()[1],VG->estimate()[2]);
    const IMU::Bias db = mpInt->GetDeltaBias(b);

    // 陀螺仪的偏置改变量
    Eigen::Vector3d dbg;
    dbg << db.bwx, db.bwy, db.bwz;

    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;   // Ri
    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();      // Ri.t()
    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;   // Rj
    const Eigen::Matrix3d Rwg = VGDir->estimate().Rwg;  // Rwg
    Eigen::MatrixXd Gm = Eigen::MatrixXd::Zero(3,2);
    Gm(0,1) = -IMU::GRAVITY_VALUE;
    Gm(1,0) = IMU::GRAVITY_VALUE;
    const double s = VS->estimate();
    const Eigen::MatrixXd dGdTheta = Rwg*Gm; // ??
    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b).cast<double>();
    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;        // r△Rij
    const Eigen::Vector3d er = LogSO3(eR);                      // r△φij
    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);  // Jr^-1(log(△Rij))

    // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度
    // Jacobians wrt Pose 1
    // _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导
    _jacobianOplus[0].setZero();
    // rotation
    // (0,0)起点的3*3块表示旋转残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1;
    // (3,0)起点的3*3块表示速度残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(s*(VV2->estimate() - VV1->estimate()) - g*dt));
    // (6,0)起点的3*3块表示位置残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(s*(VP2->estimate().twb - VP1->estimate().twb
                                                   - VV1->estimate()*dt) - 0.5*g*dt*dt));
    // translation
    // (6,3)起点的3*3块表示位置残差对pose1的位置求导
    _jacobianOplus[0].block<3,3>(6,3) = Eigen::DiagonalMatrix<double,3>(-s,-s,-s);

    // Jacobians wrt Velocity 1
    // _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导
    _jacobianOplus[1].setZero();
    _jacobianOplus[1].block<3,3>(3,0) = -s*Rbw1;
    _jacobianOplus[1].block<3,3>(6,0) = -s*Rbw1*dt;

    // Jacobians wrt Gyro bias
    // _jacobianOplus[2] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导
    _jacobianOplus[2].setZero();
    _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg;
    _jacobianOplus[2].block<3,3>(3,0) = -JVg;
    _jacobianOplus[2].block<3,3>(6,0) = -JPg;

    // Jacobians wrt Accelerometer bias
    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导
    _jacobianOplus[3].setZero();
    _jacobianOplus[3].block<3,3>(3,0) = -JVa;
    _jacobianOplus[3].block<3,3>(6,0) = -JPa;

    // Jacobians wrt Pose 2
    // _jacobianOplus[3] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
    _jacobianOplus[4].setZero();
    // rotation
    _jacobianOplus[4].block<3,3>(0,0) = invJr;
    // translation
    _jacobianOplus[4].block<3,3>(6,3) = s*Rbw1*Rwb2;

    // Jacobians wrt Velocity 2
    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
    _jacobianOplus[5].setZero();
    _jacobianOplus[5].block<3,3>(3,0) = s*Rbw1;

    // Jacobians wrt Gravity direction
    // _jacobianOplus[3] 9*2矩阵 总体来说就是三个残差分别对重力方向求导
    _jacobianOplus[6].setZero();
    _jacobianOplus[6].block<3,2>(3,0) = -Rbw1*dGdTheta*dt;
    _jacobianOplus[6].block<3,2>(6,0) = -0.5*Rbw1*dGdTheta*dt*dt;

    // Jacobians wrt scale factor
    // _jacobianOplus[3] 9*1矩阵 总体来说就是三个残差分别对尺度求导
    _jacobianOplus[7].setZero();
    _jacobianOplus[7].block<3,1>(3,0) = Rbw1*(VV2->estimate()-VV1->estimate()) * s;
    _jacobianOplus[7].block<3,1>(6,0) = Rbw1*(VP2->estimate().twb-VP1->estimate().twb-VV1->estimate()*dt) * s;
}

void EdgePriorAcc::linearizeOplus()
{
    // Jacobian wrt bias
    _jacobianOplusXi.block<3,3>(0,0) = Eigen::Matrix3d::Identity();

}

void EdgePriorGyro::linearizeOplus()
{
    // Jacobian wrt bias
    _jacobianOplusXi.block<3,3>(0,0) = Eigen::Matrix3d::Identity();

}

// SO3 FUNCTIONS
Eigen::Matrix3d ExpSO3(const Eigen::Vector3d &w)
{
    return ExpSO3(w[0],w[1],w[2]);
}

Eigen::Matrix3d ExpSO3(const double x, const double y, const double z)
{
    const double d2 = x*x+y*y+z*z;
    const double d = sqrt(d2);
    Eigen::Matrix3d W;
    W << 0.0, -z, y,z, 0.0, -x,-y,  x, 0.0;
    if(d<1e-5)
    {
        Eigen::Matrix3d res = Eigen::Matrix3d::Identity() + W +0.5*W*W;
        return NormalizeRotation(res);
    }
    else
    {
        Eigen::Matrix3d res =Eigen::Matrix3d::Identity() + W*sin(d)/d + W*W*(1.0-cos(d))/d2;
        return NormalizeRotation(res);
    }
}

Eigen::Vector3d LogSO3(const Eigen::Matrix3d &R)
{
    const double tr = R(0,0)+R(1,1)+R(2,2);
    Eigen::Vector3d w;
    w << (R(2,1)-R(1,2))/2, (R(0,2)-R(2,0))/2, (R(1,0)-R(0,1))/2;
    const double costheta = (tr-1.0)*0.5f;
    if(costheta>1 || costheta<-1)
        return w;
    const double theta = acos(costheta);
    const double s = sin(theta);
    if(fabs(s)<1e-5)
        return w;
    else
        return theta*w/s;
}

Eigen::Matrix3d InverseRightJacobianSO3(const Eigen::Vector3d &v)
{
    return InverseRightJacobianSO3(v[0],v[1],v[2]);
}

Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const double z)
{
    const double d2 = x*x+y*y+z*z;
    const double d = sqrt(d2);

    Eigen::Matrix3d W;
    W << 0.0, -z, y,z, 0.0, -x,-y,  x, 0.0;
    if(d<1e-5)
        return Eigen::Matrix3d::Identity();
    else
        return Eigen::Matrix3d::Identity() + W/2 + W*W*(1.0/d2 - (1.0+cos(d))/(2.0*d*sin(d)));
}

Eigen::Matrix3d RightJacobianSO3(const Eigen::Vector3d &v)
{
    return RightJacobianSO3(v[0],v[1],v[2]);
}

Eigen::Matrix3d RightJacobianSO3(const double x, const double y, const double z)
{
    const double d2 = x*x+y*y+z*z;
    const double d = sqrt(d2);

    Eigen::Matrix3d W;
    W << 0.0, -z, y,z, 0.0, -x,-y,  x, 0.0;
    if(d<1e-5)
    {
        return Eigen::Matrix3d::Identity();
    }
    else
    {
        return Eigen::Matrix3d::Identity() - W*(1.0-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
    }
}

Eigen::Matrix3d Skew(const Eigen::Vector3d &w)
{
    Eigen::Matrix3d W;
    W << 0.0, -w[2], w[1],w[2], 0.0, -w[0],-w[1],  w[0], 0.0;
    return W;
}

}